/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation for RangingClient.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "RangingClient.hpp"
#include "binbag.h"
#include "timestamp.hpp"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************

//*****************************************************************************
using namespace MaCI::Ranging;
//*****************************************************************************
//*****************************************************************************

CRangingClient::CRangingClient(gimi::GIMI *aGIMIPtr, const int aInterfaceMinor)
  : CSync::CSync(3,1), 
    CThread::CThread(1),
    CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_RANGING, aInterfaceMinor),
    iMessageSequence(0),
    iLimits(),
    iGotLimits(false),
    iPosition(),
    iGotPosition(false),
    iDistanceArray(),
    iDistanceHeader(),
    iDistanceTimestamp(),
    iLastDistanceArraySequence(-1),
    iRangingClientCallback(NULL)
{
  dPrint(4,"CRangingClient %p constructed", this);
}
//*****************************************************************************

CRangingClient::~CRangingClient()
{
  DoClose();
  dPrint(4,"CRangingClient %p destructed", this);
}
//*****************************************************************************

MaCI::EMaCIError CRangingClient::DoOpen(void)
{
  MaCI::EMaCIError result = KMaCIError;

  if (IsThreadRunning() == false) {
    
    // Subscribe to target. This also verifies the service existence.
    if ((result = SubscribeToInterface(TimeoutValue(1000), 10000) ) != KMaCIOK) {
      dPrint(2,"Failed to Subscribe to RangingServer (%d:%d) at target '%s'",
             iInterfaceMajor,
             iInterfaceMinor,
             iDefaultTarget.c_str());
      
    } else {
      dPrint(4,"Succesfully Subscribed to RangingServer (%d:%d) at target '%s'",
             iInterfaceMajor,
             iInterfaceMinor,
             iDefaultTarget.c_str());
      
    }
    
    // If inits were OK.
    if (result == KMaCIOK) {
      EnableSynchronousMessageReply();

      // Start the processing thread.
      SetActive(true);

      // Enable sync receive.
      CThread::RunThread();
    }
  }
  

  return result;
}
//*****************************************************************************

MaCI::EMaCIError CRangingClient::DoClose(void)
{
  MaCI::EMaCIError result = KMaCIError;

  if (IsThreadRunning() == true) {
    SetActive(false);
    UnsubscribeToInterface();
    
    CThread::WaitThread();

    result = KMaCIOK;
  }

  return result;
}
//*****************************************************************************

bool CRangingClient::GetDistanceArray(CRangingData &aData,
                                      int *aSequence, const int aTimeout_ms)
{
  // Temporary holders
  TDistanceArray da;
  TDistanceHeader dh;
  Common::TTimestamp ts;

  // Do the real call to fetcher function.
  bool r = GetDistanceArray(da, &dh, &ts, aSequence, aTimeout_ms);

  // If succesfully, create a container from this data.
  if (r) {
    // Construct the container.
    dPrint(8,"Constructing container version of data.");
    // If there is no BinBag backend, create one.
    bool f;
    aData.Reset();
    aData.CreateInternalBinBag();
    f = aData.SetTimestamp(ts); assert(f == true);
    f = aData.SetDistanceHeader(dh); assert(f == true);
    for(EACH_IN_i(da)) {
      f = aData.AddDistance(*i); assert(f == true);
    }
  }
  
  // Return the result.
  return r;
}
//*****************************************************************************

bool CRangingClient::GetDistanceArray(TDistanceArray &aArray, 
                                      TDistanceHeader *aHeader, 
                                      Common::TTimestamp *aTimestamp,
                                      int *aSeq, const int aTimeout_ms)
{
  bool result = false;

  Lock();
  
  if (aSeq == NULL) {
    // No sequence number defined. Just do direct copy of local data - no sync
    aArray = iDistanceArray;
    if (aHeader != NULL) *aHeader = iDistanceHeader;
    if (aTimestamp != NULL) *aTimestamp = iDistanceTimestamp;
    result = true;
    
  } else {
    
    if (iLastDistanceArraySequence <= *aSeq) {
      
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();

      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left, KEventDistanceArray);
        time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
	
      } while(IsActive() && iLastDistanceArraySequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iLastDistanceArraySequence > *aSeq) {
        
        // Yes :)
        aArray = iDistanceArray;
        if (aHeader != NULL) *aHeader = iDistanceHeader;
        if (aTimestamp != NULL) *aTimestamp = iDistanceTimestamp;
	
        *aSeq = iLastDistanceArraySequence;
        result = true;
	
      } else {
        // No :(
        dPrint(3, "Failed to get DistanceArray! (Timeout %dms)",
               aTimeout_ms);
        
      }
      
    } else {
    
      // The available scan is newer or matching age than requested.
      aArray = iDistanceArray;
      if (aHeader != NULL) *aHeader = iDistanceHeader;
      if (aTimestamp != NULL) *aTimestamp = iDistanceTimestamp;
      *aSeq = iLastDistanceArraySequence;
      result = true;
    }
  }
  
  Unlock();
  
  return result;
}
//*****************************************************************************

bool CRangingClient::GetDeviceLimits(TDeviceInformationLimits &aLimits, 
                                     const int aTimeout, const bool aDirect)
{
  bool result = false;
  
  if (iGotLimits && !aDirect) {
    // We already have the limits, and refresh wasn't ordered. So just return
    aLimits = iLimits;
    result = true;
    
  } else {
    CRangingData command;
    command.CreateInternalBinBag();
    command.AddCommand(MaCI::Ranging::TCommand(KCommandQueryLimits));

    // Get message seq.
    const unsigned int msgid = GetUniqueMessageID();
    
    // Send message to Request.
    SendInterfaceMessage(command.GetBinBagContainerPtr()->GetBuffer(),
                         command.GetBinBagContainerPtr()->GetBufferSize(),
                         msgid);

    // Get reply.
    CRangingData reply;
    SMessageSource source;
    if (ReceiveSynchronousMessageReply(reply, source, msgid, aTimeout) == KMaCIOK) {
      //      dPrint(ODTEST,"Got reply!!");
      
      // Got reply, lock container until processed.
      Lock();
      
      // Check whether it contains the correct data element.
      const TDeviceInformationLimits *lim = reply.GetDeviceInformationLimits();
      if (lim) {
        iLimits = *lim; // Store to class instance for next cached call.
        aLimits = *lim; // Store to caller as reply.
        Notify(KEventLimits); // Not really used anywhere currently.

        if (!iGotLimits) iGotLimits = true;
        
        result = true;
      } else {
        dPrint(ODERROR,"INTERNAL ERROR: Received reply but it didn't contain correct reply element!");
        
      }
      
      Unlock();
    } 
  }
  return result;
}
//*****************************************************************************

bool CRangingClient::GetDevicePosition(TDeviceInformationPosition &aPosition,
                                       const int aTimeout, const bool aDirect)
{
  bool result = false;
  if (iGotPosition && !aDirect) {
    aPosition = iPosition;
    result = true;
    
  } else {
    CRangingData command;
    command.CreateInternalBinBag();
    command.AddCommand(MaCI::Ranging::TCommand(KCommandQueryPosition));
    
    // Get message seq.
    const unsigned int msgid = GetUniqueMessageID();
    
    // Send message to Request.
    SendInterfaceMessage(command.GetBinBagContainerPtr()->GetBuffer(),
                         command.GetBinBagContainerPtr()->GetBufferSize(),
                         msgid);
    
    // Get reply.
    CRangingData reply;
    SMessageSource source;
    if (ReceiveSynchronousMessageReply(reply, source, msgid, aTimeout) == KMaCIOK) {
      
      // Got reply, lock container until processed.
      Lock();
      
      // Check whether it contains the correct data element.
      const TDeviceInformationPosition *pos = reply.GetDeviceInformationPosition();
      if (pos) {
        iPosition = *pos; // Store to class instance for next cached call.
        aPosition = *pos; // Store to caller as reply.
        Notify(KEventPosition); // Not really used anywhere currently.
        
        if (!iGotPosition) iGotPosition = true;
        
        result = true;
      } else {
        dPrint(ODERROR,"INTERNAL ERROR: Received reply but it didn't contain correct reply element!");
        
      }
      
      Unlock();
    } 
  }
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int CRangingClient::ThreadFunction(int aThreadNumber)
{
  int p = 0;
  int result = 0;
  int r;
  gimi::GIMIMessage gmsg;

  assert(aThreadNumber == 0);
  
  dPrint(3,"GIMIMessage handler started");

  // Run as long as the thread should be active
  while(IsActive()) {
    r = iGIMIPtr->receive(gmsg,
                          1000*60*60*24,
                          iInterfaceMajor,
                          iInterfaceMinor,
                          iRedirectionBufferId);
    switch(r) {
    case GIMI_OK:
      // Got GIMI OK, so process message.
      p = ProcessMessage(gmsg);
      break;

    case GIMI_TIMEOUT:
      // GIMI Timeout - Do nothing.
      break;

    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      if(IsActive()){
        // Other error, try to recover by sleeping one second.
        dPrint(3,"ProcessMessage returned error and client still active. Trying to recover by sleeping 1000ms ");
        ownSleep_ms(1000);
      }else{
        //
        dPrint(3,"ProcessMessage returned error. Normal situtation because client is already closed");
      }
      /*
      // Any error, print warning and sleep for one second.
      dPrint(2,"GIMI Error (%d, '%s') received. Aborting!",
             r, gimi::getGimiErrorString(r).c_str());
             SetActive(false);*/
      break;
    }
    TestCancel();
  }

  dPrint(3,"GIMIMessage handler stopped");

  return result;
}
//*****************************************************************************

int CRangingClient::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CRangingData rd;
  bool r;

  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    r = rd.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as Ranging, now handle contents
      // Check for DistanceHeader & Distances
      const TDistanceHeader *hdr = rd.GetDistanceHeader();
      if (hdr) {
        // Got distanceheader
        if (rd.GetDistanceElementCount() == (int)hdr->beam_count) {
          // Got Distances too!
          Lock();
          //iDistanceTimestamp = rd.GetTimestamp() ? (*rd.GetTimestamp()) : Common::TTimestamp();
          if (rd.GetTimestamp()) {
            iDistanceTimestamp = *rd.GetTimestamp();

          } else {
            dPrint(ODWARN,"Warning: Received RangingData element with no Timestamp set!");

          }

          //         dPrint(1,"rd.GetTimestamp: %u:%u", rd.GetTimestamp()->timestamp_s, rd.GetTimestamp()->timestamp_us);
          iDistanceHeader = *hdr;
          iDistanceArray.clear();
          for (unsigned int i=0; i < hdr->beam_count; ++i) {
            const TDistance *d = rd.GetDistance(i);
            iDistanceArray.push_back(*d);
          }
          ++iLastDistanceArraySequence;
          Unlock();
          Notify(KEventDistanceArray);
          dPrint(8,"Got DistanceArray - Stored (Seq %d)", iLastDistanceArraySequence);
          
          // Call the handler if set.
          if (iRangingClientCallback) {
            dPrint(8,"Got Callback for RangingDataEvent - executing!");
            iRangingClientCallback->OnRangingDataEvent(rd,
                                                       iLastDistanceArraySequence,
                                                       *this);
            dPrint(8,"Callback for RangingDataEvent - execution complete!");
        
          }
        }
      }
    } else {
      dPrint(4,"WARNING: Received Ranging interface message, but failed to decode as RangingData!");
      
    }
  }
  
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
